/*
 * Simulation.cpp
 *
 *  Created on: Jan 23, 2011
 *      Author: Team MRF
 */


#include "Simulation.h"


//#include <iostream>
//#include <string>
#include <cmath>
//using namespace std;

const float PI = 3.141592654;
const float GRAVITY = -9.8;

namespace gsp280Project2
{

	//=========================
	//CLASS METHODS
	//=========================
	float DEG2RAD(float a) { return float(a * (PI/180) ); }
	float RAD2DEG(float a) { return float(a * (180/PI) ); }

	//=========================
	//PRIVATE METHODS
	//=========================

	//M001:
	const float Simulation::calc_AngularMomentum() const
	{
		//AngularMomentum(L) = mass * radius^2 * angularMomentum(omega)
		return (float( mass * (radius * radius) *omega) );
	}

	//M002:
	const float Simulation::calc_TangentialVelocity() const
	{
		//Vt = radius * angular Velocity(omega)
		return ( float(radius * omega) );
	}

	//M003:
	const float Simulation::calc_Vx() const
	{
		return (float ( calc_TangentialVelocity() * cosf(angle)));
	}

	//M004:
	const float Simulation::calc_Vy() const
	{
		return (float ( calc_TangentialVelocity() * sinf(angle)));
	}

	//M005:
	const float Simulation::calc_TotalTime() const
	{
		//Find time = ?, using Vf = Vi + a.t,
		//NOTE: Since finding time in Vy ONLY gets time 1/2 way thru projectile
		//SO multiply time * 2
		if(float( (2 * (0 - Vy)) / (GRAVITY) ) < 0)
		{
			return (
					-1 * (float( (2 * (0 - Vy)) / (GRAVITY) ))
					);
		}
		else
		{
			return (
					float( (2 * (0 - Vy)) / (GRAVITY) )
					);
		}
	}

	//M006:
	const float Simulation::calc_ProjectileHeight ( )
	{
		//Find time = ?, using Vf = Vi + a.t,
		float time = (0 - Vy) / (GRAVITY);

		//Find projectile Height, using deltaY = Viy.t + 0.5 (a)(t^2)
		return (float ( (Vy * time) + ( 0.5 * (GRAVITY) * (time * time) ) ) );
	}

	//M007:
	const float Simulation::calc_ProjectileRange()
	{
		//Range formula: deltaX = Vx * time
		return ( float(Vx * totalTime) );
	}

	//M008: use this method ONLY if M007 output >= (targetObj.X )
	const Vector2 Simulation::calc_FlyingObj_NewLocation()
	{
		if ( projectileRange > targetObj.getLocation_X() )
		{
			//Using TotalTime variable to find (x,y) of flying obj

			//using deltaX = Initial Horizontal Velocity(Vx) * time
			//Use the targetObj's x-Location as launch Obj's x-location
			float time = (targetObj.getLocation_X() / Vx);
			
			//using the time it took the flying Obj to reach location of the target obj
			//Find projectile Height, using deltaY = Viy * t + 0.5 (a)(t^2) @ t= time
			float tempY = ( (Vy * time) + ( 0.5 * (GRAVITY) * (time * time) ) );

			//return location of launch Obj closest to target Obj
			return Vector2(targetObj.getLocation_X(), tempY);
		}
		else { return Vector2(projectileRange, 0); }
	}

	//M009:
	// calculate the new angular momentum (ogema)
	// to adjust the trajectory of the launch object
	void Simulation::calc_NewAngularVelocity()
	{
		float xtraOmega;

		Vector2 l_obj, t_obj;
		l_obj = launchObj.getLocation();
		t_obj = targetObj.getLocation();

		//Check if the launched object has fell under the Target object
		//if(launchObj.getLocation_X() < targetObj.getLocation_X())
		if(l_obj.getX() < t_obj.getX())
		{
			//xtraOmega = (Target_X - Launch_X) / 2
			xtraOmega = ((t_obj.getX() - l_obj.getX()) / 2);

			//xtraOmega = (xtraOmega * omega) / Launch_X = Extra omega needed to travel the needed distance
			xtraOmega = (xtraOmega * omega)/l_obj.getX();

			// omega = omega + extra omega
			omega += xtraOmega;
		}
		//Check if the launched object has flown over the Target object
		//else if(launchObj.getLocation_X() > targetObj.getLocation_X())
		else if(l_obj.getX() > t_obj.getX())
		{
			//xtraOmega = (Target_X - Launch_X) / 2
			xtraOmega = ((l_obj.getX() - t_obj.getX()) / 2);

			//xtraOmega = (xtraOmega * omega) / Launch_X = Extra omega needed to travel the needed distance
			xtraOmega = (xtraOmega * omega)/t_obj.getX();

			//omega = omega - extra omega
			omega -= xtraOmega;
		}
		//If X-Axis of Launched Object and Target Object are same but Y-Axis is under
		//else if((launchObj.getLocation_X() == targetObj.getLocation_X()) && (launchObj.getLocation_Y() < targetObj.getLocation_Y()))
		else if(l_obj.getX() == t_obj.getX() && l_obj.getY() < t_obj.getY())
		{
			//xtraOmega = (Target_X - Launch_X) / 2
			xtraOmega = ((t_obj.getY() - l_obj.getY()) / 2);

			//temp = (xtraOmega * omega) / Launch_X = Extra omega needed to travel the needed distance
			xtraOmega = (xtraOmega * omega)/l_obj.getY();

			omega += xtraOmega;	//omega = omega + extra omega
		}
		//If X-Axis of Launched Object and Target Object are same but Y-Axis is over
		//else if((launchObj.getLocation_X() == targetObj.getLocation_X()) && (launchObj.getLocation_Y() > targetObj.getLocation_Y()))
		else if((l_obj.getX() == t_obj.getX()) && (l_obj.getY() > t_obj.getY()))
		{
			//xtraOmega = (Target_X - Launch_X) / 2
			xtraOmega = ((l_obj.getY() - t_obj.getY()) / 2);

			//xtraOmega = (temp * omega) / Launch_X = Extra omega needed to travel the needed distance
			xtraOmega = (xtraOmega * omega)/t_obj.getY();

			//omega = omega - extra omega
			omega -= xtraOmega;
		}
	}

	//M010: Gets called repeatedly until a successful collision has been detected
	void Simulation::calc_Simulation()
	{
		// calculate the new radius
		radius = sqrt(angMomentum/(mass * omega));
		
		// calculate new tangential velocity
		Vt = calc_TangentialVelocity();	

		// calculate new X and Y velocities
		Vx = calc_Vx();
		Vy = calc_Vy();

		// calculate the new time of flight
		totalTime = calc_TotalTime();

		// calculate the projectiles new height
		projectileHeight = calc_ProjectileHeight();

		// calculate the projectiles new range/distance
		projectileRange = calc_ProjectileRange();

		//Update the launchObj Location
		launchObj.setLocation(calc_FlyingObj_NewLocation());
	
		// NEXT STAGE WILL CHECK FOR COLLISION HERE
	}
	
	//M011: Continue to run and loop until a collision is detected
	void Simulation::SimLoop()
	{
		//cout << counter <<"\t"; //cout << launchObj.intersects(targetObj);

		do{
			//Get new angular Velocity
			calc_NewAngularVelocity();

			//Complete necessary calculations
			calc_Simulation();

			++counter;  //display();

		}while(!launchObj.intersects(targetObj));//!hasCollided);

		//Once Collision is detected, perform deformation on target Obj
		calc_ShearStress();
	}

	//M013: Display Results from Simulation
	void Simulation::display ( )
	{
		cout << "\n==============================================================\n" << endl;

		cout << "Simulation loop counter: " << counter << "\n" << endl;

		cout << "Angle in radians: " << angle << endl;
		cout << "Angle in degrees: " << RAD2DEG(angle) << endl << endl;

		cout << "Angular Momentum: " << angMomentum << endl;
		cout << "Radius of Catapult: " << radius << endl;
		cout << "Angular Velocity (omega): " << omega << endl;
		cout << "Tangential Velocity: " << Vt << endl << endl;

		cout << "Initial Velocity (X-Axis): " << Vx << endl;
		cout << "Initial Velocity (Y-Axis): " << Vy << endl;
		cout << "Total Time of flight: " << totalTime << endl << endl;

		cout << "Max Projectile Height: " << projectileHeight << endl;
		cout << "Range/Distance of Projectile: " << projectileRange << endl << endl;

		cout << "Launch Object Location: (" << launchObj.getLocation_X() << " , " << launchObj.getLocation_Y() << ")" << endl;
		cout << "Target Object Location: (" << targetObj.getLocation_X() << " , " << targetObj.getLocation_Y() << ")" << endl << endl;

		cout << "==============================================================\n" << endl;
	}


	//M014: Calculate Shear Stress, once collision is detected
	void Simulation::calc_ShearStress()
	{
		//code goes here....
		//001: Find Viy using, dY = Viy.t + 0.5.a.t^2
		Vy = targetObj.getLocation_X() - (0.5 * GRAVITY * totalTime);

		//002: Find Vt, using Pytheous Theorem
		Vt = sqrtf( (Vx * Vx) + (Vy * Vy) );

		//003: Find Theta, in Radians...
		tAngle = atanf( Vy/Vx );

		//004: Find Shear Stress Displacement
		//A: Convert tAngle to degrees first
		tAngle = RAD2DEG(tAngle);
		shearDisplacement =
				( launchObj.getLocation_Y() - targetObj.getLocation_Y() )
				* tanf(tAngle);

		//005: Update Sheared Vertices
		targetObj.setVertex_BL( targetObj.getLocation_X() + shearDisplacement, targetObj.getLocation_Y() );
		targetObj.setVertex_BR( (targetObj.getVertex_BL().getX() + targetObj.getCollisionBox_Width()), targetObj.getLocation_Y() );
		targetObj.setVertex_TL( targetObj.getLocation_X(), (targetObj.getLocation_Y() + targetObj.getCollisionBox_Height()) );
		targetObj.setVertex_TR( targetObj.getLocation_X() + targetObj.getCollisionBox_Height(), targetObj.getVertex_TL().getY() );
	}


	//=========================
	//PUBLIC METHODS
	//=========================

	//M012: Start Simulation
	// add a Stop() method as well
	void Simulation::Start ( const pShort & mass, const float & radius,
							const float & omega, const float & angle, const Vector2 &vecLaunch,
							const Vector2 &vecTarget )
	{
		counter = 0;

		this->mass = mass;
		this->radius = radius;
		this->omega = omega;

		//Track Init Values from User Input
		initMass = mass;
		initRadius = radius;
		initOmega = omega;
		initAngle = angle;
		initLaunchObj_Loc = vecLaunch;
		initTargetObj_Loc = vecTarget;

		launchObj.setLocation(vecLaunch); //set the location of launch obj
		launchObj.setRadius(radius);

		targetObj.setLocation(vecTarget); //set the location of target obj
		targetObj.setRadius(radius);

		//Track Initial Vertices of Target Obj
		targetObj.setVertex_BL( targetObj.getCollisionBox_Location_X(), targetObj.getCollisionBox_Location_Y() );
		targetObj.setVertex_BR( (targetObj.getCollisionBox_Location_X() + targetObj.getCollisionBox_Width()), targetObj.getCollisionBox_Location_Y() );
		targetObj.setVertex_TL( targetObj.getCollisionBox_Location_X(), (targetObj.getCollisionBox_Location_Y() + targetObj.getCollisionBox_Height()) );
		targetObj.setVertex_TR( (targetObj.getCollisionBox_Location_X() + targetObj.getCollisionBox_Width()), (targetObj.getCollisionBox_Location_Y() + targetObj.getCollisionBox_Height()) );

		initTargetObj_BL = targetObj.getVertex_BL();
		initTargetObj_BR = targetObj.getVertex_BR();
		initTargetObj_TL = targetObj.getVertex_TL();
		initTargetObj_TR = targetObj.getVertex_TR();

		// angle is converted to radians because the cosf() function takes
		// in radians within its parameter.
		this->angle = DEG2RAD(angle);


		angMomentum = calc_AngularMomentum();
		Vt = calc_TangentialVelocity();				// Launching Velocity at a specific angle

		Vx = calc_Vx();								// Flying objects initial X-Axis velocity
		Vy = calc_Vy();								// Flying objects initial Y-Axis velocity

		totalTime = calc_TotalTime();				// Total time the Object was in flight
		projectileHeight = calc_ProjectileHeight();	// Max projectile height(vertex)
		projectileRange = calc_ProjectileRange();	// Range/Distance of projectile

		launchObj.setLocation(calc_FlyingObj_NewLocation()); // Objects "closest point" to target

		SimLoop();
	}

	// M016: Stop Simulation
	void Simulation::Stop()
	{
		cout << "\nThank you for trying this humble simulation\nGood Bye!\n\n" << endl;
		exit(1);
	}

	// M015: Display the final summary of results
	void Simulation::displaySummary()
	{
		cout << "*************** INITIAL INPUT FROM USER ***************" << endl << endl;
		cout << "\tInitial Mass of Object: " << initMass << endl;
		cout << "\tInitial Radius of Catapult: " << initRadius << endl;
		cout << "\tInitial Angular Velocity(Omega): " << initOmega << endl;
		cout << "\tInitial Launch Angle: " << initAngle << endl;
		cout << "\tInitial Launch Object Location: (" << initLaunchObj_Loc.getX() << ", " << initLaunchObj_Loc.getY() << ")" << endl;
		cout << "\tInitial Target Object Location: (" << initTargetObj_Loc.getX() << ", " << initTargetObj_Loc.getY() << ")" << endl << endl;

		cout <<"\tInitial Target Object Vertices:" << endl;
		cout << "\tBL: [" << targetObj.float_RoundOff2( initTargetObj_BL.getX() ) << ", "  << targetObj.float_RoundOff2( initTargetObj_BL.getY() ) << "]";
		cout << "\tBR: [" << targetObj.float_RoundOff2( initTargetObj_BR.getX() ) << ", "  << targetObj.float_RoundOff2( initTargetObj_BR.getY() ) << "]";
		cout << "\tTL: [" << targetObj.float_RoundOff2( initTargetObj_TL.getX() ) << ", "  << targetObj.float_RoundOff2( initTargetObj_TL.getY() ) << "]";
		cout << "\tTR: [" << targetObj.float_RoundOff2( initTargetObj_TR.getX() ) << ", "  << targetObj.float_RoundOff2( initTargetObj_TR.getY() ) << "]";
		cout << endl << endl;


		cout << "*************** FINAL OUTPUT FROM SIMULATION ***************" << endl << endl;
		cout << "\tSimulation loop ran " << counter << " times\n\n";

		cout << "\tFinal Angular Momentum: " << angMomentum << " kg.m^2.rad/s " << endl;
		cout << "\tFinal Radius of Catapult: " << launchObj.float_RoundOff2( radius ) << " meters" << endl << endl;

		cout << "\tFinal Angular Velocity (omega): " << launchObj.float_RoundOff2( omega ) << " rad/s" << endl;
		cout << "\tFinal Tangential Velocity: " << launchObj.float_RoundOff2( Vt ) << " m/s" << endl;
		cout << "\n\tVelocity (X-Axis): " << launchObj.float_RoundOff2( Vx ) << " m/s" << endl;
		cout << "\tVelocity (Y-Axis): " << launchObj.float_RoundOff2( Vy ) << " m/s" << endl << endl;

		cout << "\tTotal Time of flight(in seconds): " << launchObj.float_RoundOff2( totalTime ) << " seconds" << endl;
		cout << "\tMax. Projectile Height: " << launchObj.float_RoundOff2( projectileHeight ) << " meters" << endl;
		cout << "\tRange/Distance of Projectile: " << launchObj.float_RoundOff2( projectileRange ) << " meters" << endl << endl;

		cout << "\tFinal Launch Object Location: (" << launchObj.getLocation_X() << " , " << launchObj.float_RoundOff2( launchObj.getLocation_Y() ) << ")" << endl;
		cout << "\tFinal Target Object Location: (" << targetObj.getLocation_X() << " , " << targetObj.getLocation_Y() << ")" << endl << endl;

		cout << "\tShear Displacement Distortion on Target \n\tObject (in the X-axis): " << launchObj.float_RoundOff2( shearDisplacement ) << " meters" << endl << endl;
		cout <<"\tFinal Target Object Vertices:" << endl;
		cout << "\tBL: [" << targetObj.float_RoundOff2( targetObj.getVertex_BL().getX() ) << ", "  << targetObj.float_RoundOff2( targetObj.getVertex_BL().getY() ) << "]";
		cout << "\tBR: [" << targetObj.float_RoundOff2( targetObj.getVertex_BR().getX() ) << ", "  << targetObj.float_RoundOff2( targetObj.getVertex_BR().getY() ) << "]";
		cout << "\n\tTL: [" << targetObj.float_RoundOff2( targetObj.getVertex_TL().getX() ) << ", "  << targetObj.float_RoundOff2( targetObj.getVertex_TL().getY() ) << "]";
		cout << "\t\tTR: [" << targetObj.float_RoundOff2( targetObj.getVertex_TR().getX() ) << ", "  << targetObj.float_RoundOff2( targetObj.getVertex_TR().getY() ) << "]";
		cout << endl << endl;

		cout << "*******************************************************" << endl << endl;
	}

}//namespace
